/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/include/agvWorld/path.h
 * @Description  : 定义路径相关的类，包括路径的类型、路径的创建、路径的索引等功能。 
 *                 包含线型路径、Bezier路径和侧移路径的实现。 
 *                 提供路径的导航、引导和姿态获取等接口。  
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 15:37:13
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#pragma once
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include "node.h"
#include "trajectory.h"
#include "bezier_traj.h"
#include <map>
#include <mutex>
// Defines NULL ID of nodes and paths
#define NULLID ((USHORT)0xFFFF)

// Enumeration of standard path types
enum class PathType
{
    LINE_TYPE = 0,
    BEZIER_TYPE = 1,
    SIDE_TYPE = 2
};

// Enumeration of path guidance type
constexpr USHORT LASER_GUIDANCE = 0;
constexpr USHORT QR_CODE_GUIDANCE = 1;
constexpr USHORT MAG_GUIDANCE = 2;
constexpr USHORT REFLECTOR_GUIDANCE = 3;
constexpr USHORT NO_GUIDANCE = 7;
constexpr float MAX_DEAD_DIST = 0.5f;

#define STR_LASER_TYPE "laser"
#define STR_QR_CODE_TYPE "qrCode"
#define STR_MAG_TYPE "magnet"
#define STR_REFLECTOR_TYPE "reflector"
#define STR_NO_GUIDE_TYPE "noGuide"
//////////////////////////////////////////////////////////////////////////////
//   The interface of class "Path".
class Path
{
 public:
    // The constructor
    Path(USHORT id, USHORT start_node, USHORT end_node, float velocity_limit,
         MoveDirTag move_dir = MoveDirTag::Forward, PathType type = PathType::LINE_TYPE,
         USHORT guide_type = LASER_GUIDANCE, float max_dead_dist = 0.0f);

    // The default constructor
    Path() { InitParam(); }

    virtual ~Path() = default;

    // Get the guide method on this path
    USHORT GuideType() const;

    // Get the pointer to the start node
    Node GetStartNode() const;

    // Get the pointer to the end node
    Node GetEndNode() const;

    // Get the world point of the start node
    Point GetStartPoint() const;

    // Get the world point of the end node
    Point GetEndPoint() const;

    // Test whether 2 path objects are equal
    bool operator==(Path &path) const;

    // Test whether 2 path objects are not equal
    bool operator!=(Path &path) const;

    // Determine the vehicle's heading angle at the specified node
    virtual Angle& GetHeading(Node node) = 0; // change Node& -> Node

    // Get the size of the path
    virtual float Size() const;

    // Make a trajectory from the path
    virtual Trajectory* MakeTraj() = 0;

    // Fuzzy node checking limit (0.03m)
    virtual float FuzzyNodeCheckLimit() const;

    // Find the standard posture at the specified progress point
    virtual bool GetPosture(float relative_progress, Posture& posture);

    // Create the path
    virtual bool Create();

    // Find guide type by string type define
    bool ConvertGuideType(const std::string& type, USHORT& guide_type);

    // 前端配置16比特位 特殊使用
    uint16_t Bits() const;

    // 通用字段1和字段2
    void Words(int &word1, int &word2) const;

    // 获得路径上避障方案
    int GetDetectPlan() const;
 public:
    USHORT id_;                  // Path ID number
    USHORT type_;                // Path topological type;TPathType
    USHORT start_node_;          // The ID of the start node
    USHORT end_node_;            // The ID of the end node
    MoveDirTag move_dir_;        // FIXME:add by yxj 2022-9-19 非全方位移动时才生效的属性
    float size_;                 // Size of the path
    float velocity_limit_;       // The velocity limit on the path
    float velocity_limit_load_;  // The velocity limit on the path for loading
    USHORT guide_type_;          // The guide method on this path
    std::string guide_type_str_; // Only for  change loc mode to guide type(laser or qrcode)
    float max_dead_dist_;        // Max dead distance:  Navigation parameter
    float stop_dist_empty_;      // Empty load stop distance (unit:m)
    float stop_dist_full_;       // Full load stop distance (unit:m)
    int detect_plan_;            // Path laser obstacle area

    uint16_t bits_;              // 前端配置16比特位
    int word1_;                  // 通用字段1
    int word2_;                  // 通用字段2
    float ang_head_;             // Radian :only for Omnidirectional movement agv from map.xml

    static NodeBase *node_base_; // Pointer to the nodes data base 

 protected:
    Angle heading_angle_;        // The vehicle's heading at the start/end nodes

 private:
    // Initialize the parameters
    void InitParam();
    std::map<std::string, USHORT> guide_map_;
};

//////////////////////////////////////////////////////////////////////////////
//   The interface of class "LinePath" - a class defining line-type paths.
class LinePath : public Path
{
 public:
    // The constructor
    LinePath(USHORT id, USHORT start_node, USHORT end_node,
           float velocity_limit = 0.6f, USHORT guide_type = LASER_GUIDANCE, 
           MoveDirTag move_dir = MoveDirTag::Forward);

    // The default constructor
    LinePath() = default;

    ~LinePath() override = default;

    // Get the vehicle's required heading at the specified node
    Angle& GetHeading(Node node) override;

    // Make a trajectory from the path
    Trajectory* MakeTraj() override;

    bool Create() override;

 private:
    // Angle m_angHeading; // The vehicle's heading at the start/end nodes
    void Init();
};

//////////////////////////////////////////////////////////////////////////////
//   Class "CBezierPath": A class derived from base class "Path", defining
//   Bezier-type paths.
//
class BezierPath : public Path
{
 public:
    // The default constructor
    BezierPath();

    BezierPath(USHORT id, USHORT start_node, USHORT end_node, const Point& ctrl_point1, const Point& ctrl_point2,
                float velocity_limit, USHORT guide_type, MoveDirTag move_dir);

    ~BezierPath() override = default;

    // Get the vehicle's required heading at the specified node
    Angle& GetHeading(const Node node) override;

    // Make a trajectory from the path
    Trajectory* MakeTraj() override;

    // Fuzzy node checking limit (5 degree)
    float FuzzyNodeCheckLimit() const override;

    bool Create() override;

    // Get the first control point of the trajectory
    const Point &GetCtrlPoint1() const;

    // Get the second control point of the trajectory
    const Point &GetCtrlPoint2() const;

    // Get the operation mode of the trajectory
    bool IsShift() const;
 
 public:
    Point ctrl_points_[2];      // Control point of the curve 0:cp1 1:cp2
    USHORT is_shift_;           // Whether the operation mode is shift or translation, motion mode 0: tangent 1: shift

 protected:
    Angle start_heading_;       // Vehicle heading at the start node
    Angle end_heading_;         // Vehicle heading at the end node
    Posture start_posture_;    // Initial attitude
    Posture end_posture_;      // Termination attitude
    // Angle m_angShiftHeading; // Degree: Vehicle body translation direction angle

 private:
    void Init();
};

//////////////////////////////////////////////////////////////////////////////
//   The interface of class "TSidePath" - a class defining side-type paths.
class SidePath : public Path
{
 public:
    // The default constructor
    SidePath() = default;

    ~SidePath() override = default;

    // Get the vehicle's required heading at the specified node
    Angle& GetHeading(Node node) override;

    // Make a trajectory from the path
    Trajectory* MakeTraj() override;

    bool Create() override;

  private:
    // side scp lazyS use
    // Angle m_angHeading; // The vehicle's heading angle on the path it is necessary
    void Init();

    // Size caculation function
    float CalculateSize() const;
};

//////////////////////////////////////////////////////////////////////////////
//   The interface of class "PathIndex", which defines the content of a
//   path index entry. It is a helper class for "PathBase".
class PathIndex
{
 public:
    Path *ptr_; // Pointer to the path data

    // The default constructor
    PathIndex(): ptr_(nullptr) {}
};

//////////////////////////////////////////////////////////////////////////////
//   The interface of class "PathBase", which organises all the paths
//   into a single class.
class PathBase
{
 public:
    // The constructor
    PathBase();

    // The destructor
    virtual ~PathBase();

    // Get the specified path object (Form #1)
    Path* GetPathPointer(USHORT path_id);

    // Get the specified path object (Form #2)
    Path* GetPathPointer(USHORT node1, USHORT node2);

    // Get the specified path object (Form #2.A)
    // delete by qf
    // Path* GetPathPointer(USHORT uNode1, USHORT uNode2, CMoveDir MoveDir);

    // Get the specified path object (Form #3)
    Path* GetPathPointer(Node &node1, Node &node2);

    // Get the specified path object (Form #4)
    Path* GetPathPointer(USHORT node1, USHORT node2, float heading_at_node1);

    // Test whether the specified node pair can form a path
    bool IsPath(USHORT node1, USHORT node2);

    // Find the vehicle's posture at uNode
    USHORT GetNeighborNode(USHORT node);

    // Get the pointer to the object
    PathBase* GetPathBaseObject() { return this; }

    // Get the posture of a vehicle at a specified node
    bool GetPosture(USHORT from_node, USHORT to_node, float relative_progress, Posture& posture);

    // create path
    bool Create(const string& map_path);

    //  only change some path properties can use this function
    bool OverWritePath(USHORT node1, USHORT node2, Path *new_path);
public:
    PathIndex* path_index_;
    USHORT path_count_;

 private:
    // Clean up the memory occupied by the path data base
    void CleanUp();

    // Add a node to the nodes data base
    bool AddPath(Path *path);

    // Delete a node from the nodes data base
    bool RemovePath(USHORT id);
    
    // path mutex
    std::mutex path_mutex_;
};

